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ABSTRACT 

Unmanned  ground  vehicles  ( UGVs )  are  gaining 
importance  and  finding  increased  utility  in  both  military  and 
commercial  applications.  Although  earlier  UGV platforms  were 
typically  exclusively  small  ground  robots,  recent  efforts  started 
targeting  passenger  vehicle  and  larger  size  platforms.  Due  to 
their  size  and  speed,  these  platforms  have  significantly  different 
dynamics  than  small  robots,  and  therefore  the  existing  hazard 
avoidance  algorithms,  which  were  developed  for  small  robots, 
may  not  deliver  the  desired  performance.  The  goal  of  this  paper 
is  to  present  the  first  steps  towards  a  model  predictive  control 
(MFC)  based  hazard  avoidance  algorithm  for  large  UGVs  that 
accounts  for  the  vehicle  dynamics  through  high  fidelity  models 
and  uses  only  local  information  about  the  environment  as 
provided  by  the  onboard  sensors.  Specifically,  the  paper 
presents  the  MPC  formulation  for  hazard  avoidance  using  a 
light  detection  and  ranging  (LIDAR)  sensor  and  applies  it  to  a 
case  study  to  investigate  the  impact  of  model  fidelity  on  the 
performance  of  the  algorithm,  where  performance  is  measured 
mainly  by  the  time  to  reach  the  target  point.  Towards  this  end, 
the  case  study  compares  a  2  degrees-offreedom  (DoF)  vehicle 
dynamics  representation  to  a  14  DoF  representation  as  the 
model  used  in  MPC.  The  results  show  that  the  2  DoF  model  can 
perform  comparable  to  the  14  DoF  model  if  the  safe  steering 
range  is  established  using  the  14  DoF  model  rather  than  the  2 
DoF  model  itself.  The  conclusion  is  that  high  fidelity  models 
are  needed  to  push  autonomous  vehicles  to  their  limits  to 
increase  their  performance,  but  simulating  the  high  fidelity 
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models  online  within  the  MPC  may  not  be  as  critical  as  using 
them  to  establish  the  safe  control  input  limits. 

INTRODUCTION 

Unmanned  ground  vehicles  (UGVs)  hold  great  potential 
for  increased  safety,  performance,  and  convenience,  and  are 
therefore  attracting  interest  for  both  commercial  and  military 
applications.  Small  ground  robots  that  are  either  teleoperated  or 
semi  or  fully  autonomous  are  already  in  use.  Currently, 
particular  interest  is  in  autonomous  UGVs  that  are  at  least  the 
size  of  a  passenger  vehicle.  In  the  commercial  field,  almost  all 
major  automakers  and  even  companies  from  other  sectors  are 
pursuing  autonomous  passenger  vehicles  [1].  As  a  military 
example,  the  Autonomous  Platform  Demonstrator  (APD)  is  a 
9.3  ton  vehicle  that  has  been  used  to  develop,  integrate,  and  test 
many  next  generation  UGV  mobility  technologies  such  as 
hybrid  electric  drive  systems,  advanced  suspension  systems, 
thermal  management  systems,  and  UGV  safety  systems  [2].  It 
is  capable  of  reaching  speeds  up  to  50  miles  per  hour  (22.35 
m/s)  and  performing  maneuvers  at  those  speeds  [2]. 

One  important  problem  in  autonomous  UGVs  regardless  of 
their  size  is  hazard  avoidance,  which  refers  to  the  problem  of 
safely  maneuvering  around  obstacles  while  traveling  to  a  target 
point.  Many  hazard  avoidance  algorithms  have  been  developed 
in  the  literature  that  allow  for  fast,  continuous,  and  smooth 
motion  of  the  UGV  among  unexpected  obstacles  [3].  These 
algorithms  generate  a  hazard-avoiding  action  in  response  to 
online  sensor  signals.  Some  early  examples  include  the 
artificial  potential  field  method  [4,  5],  the  vector  field 
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histogram  method  [3,  6-10],  and  the  dynamic  window  approach 
[11,  12].  However,  these  algorithms  typically  focus  on  small 
ground  robots  and  hence  rely  on  assumptions  such  as 
representing  the  vehicle  as  a  point,  which  do  not  hold  for  larger 
UGVs.  Thus,  hazard  avoidance  algorithms  developed  for  small 
UGVs  may  not  deliver  the  desired  performance  in  larger  UGVs. 

More  recent  work  aimed  to  improve  the  algorithms 
originally  developed  for  small  robots  and  to  adapt  them  to 
large,  high-speed  UGVs.  For  example,  Shimoda  et  al.  [5]  used 
trajectory  space  based  potential  field  method  to  navigate  a  high 
speed  UGV  on  rough  terrain,  avoiding  discrete  static  hazards 
and  dynamically  inadmissible  maneuvers.  A  potential  field  is 
constructed  in  the  trajectory  space,  which  is  a  two-dimensional 
space  of  a  UGV’s  instantaneous  path  curvature  and  longitudinal 
velocity,  and  vehicle  maneuvers  are  selected  based  on  the 
properties  of  the  field.  The  potential  field  is  defined  as  a  sum  of 
potential  functions  relating  to  the  waypoint  location,  desired 
velocity,  hazard,  rollover  and  side  slip  constraints.  Although 
computationally  efficient,  this  method  does  not  guarantee 
feasibility  or  optimality  [13]. 

To  address  the  optimality  problem,  more  rigorous  methods 
have  been  pursued  leveraging  the  model  predictive  control 
(MPC)  approach.  MPC  is  a  form  of  control  in  which  the  current 
control  action  is  obtained  by  solving  a  finite  horizon  open-loop 
optimal  control  problem  using  the  current  state  of  the  plant  as 
the  initial  state;  the  optimization  yields  an  optimal  control 
sequence  and  the  first  control  in  this  sequence  is  applied  to  the 
plant.  MPC  is  a  promising  approach  for  hazard  avoidance  due 
to  its  capability  to  handle  input  saturation,  system  nonlinearity, 
and  state  constraints  in  a  dynamic  environment.  For  example, 
Tahirovic  and  Magnani  [14]  provided  a  framework  for 
incorporating  a  vehicle  model  into  navigation  by  using  an 
adapted  version  of  passivity-based  nonlinear  model  predictive 
control  to  extend  the  convergent  dynamic  window  approach 
[12].  It  can  be  considered  a  generalized  navigation  planning 
technique  able  to  include  the  high  fidelity  models  required  to 
describe  the  dynamics  of  vehicles  moving  outdoor  on  rough 
terrain. 

Even  though  this  work  and  several  others  [15-17] 
demonstrated  successful  application  of  MPC  to  hazard 
avoidance,  several  research  questions  still  remain  to  be 
addressed.  For  example,  the  passivity-based  MPC  navigation 
algorithm  assumes  the  environment  is  completely  known.  In 
reality,  only  local  information  from  onboard  sensors  will  be 
available.  Furthermore,  to  reduce  the  computational  load,  only 
simple  vehicle  dynamics  models  such  as  the  2  degrees  of 
freedom  (DoF)  representation  have  been  considered  so  far  as 
the  model  in  MPC.  As  pointed  by  Park  [15],  only  when  the 
assumptions  used  to  derive  the  simplified  model  are  satisfied, 
the  actual  vehicle  can  track  the  generated  trajectory  based  on 
this  simplified  model.  The  level  of  complexity  of  the  model  that 
needs  to  be  used  in  MPC  for  best  performance  is  unknown. 


Therefore,  the  goal  of  this  paper  is  twofold:  (1)  to  describe 
an  MPC  based  hazard  avoidance  algorithm  that  works  only 
with  local  information  about  the  environment  from  the  onboard 
sensors;  and  (2)  to  evaluate  on  a  case  study  the  performance  of 
the  algorithm  with  low  versus  high  fidelity  vehicle  models.  A 
typical  4-wheel  truck  is  considered  as  an  example  vehicle 
platform.  Regarding  the  first  goal,  the  vehicle  is  assumed  to  be 
equipped  with  a  light  detection  and  ranging  (LIDAR)  sensor. 
Regarding  the  second  goal,  the  performance  is  mainly 
measured  by  the  time  to  reach  the  target;  the  best  performance 
is  achieved  when  the  vehicle  reaches  the  target  safely  and  in 
minimum  time.  Two  different  representations  of  vehicle 
dynamics  within  MPC  are  compared;  a  2  DoF  representation  as 
a  lower  fidelity  model  and  a  14  DoF  representation  as  a  higher 
fidelity  model. 

Throughout  the  paper  it  is  assumed  that  the  computational 
power  needed  for  using  high  fidelity  models  within  MPC  can 
be  met  in  real  time  by  leveraging  the  recent  developments  in 
high  power  computing  capabilities  [18].  Thus,  computational 
aspects  of  the  algorithm  are  not  discussed  and  computational 
time  is  not  used  as  a  performance  metric. 

The  rest  of  the  paper  is  organized  as  follows.  First,  the 
MPC  based  hazard  avoidance  algorithm  is  presented.  The 
vehicle  models,  constraints,  cost  function,  and  dynamic 
optimizer  are  described  in  detail.  Simulation  results  are 
presented  next  using  two  different  obstacle  maps  to  validate  the 
proposed  approach  and  evaluate  its  performance  with  2  DoF 
versus  14  DoF  model  in  the  MPC.  Finally,  the  limitations  of 
this  study  are  outlined  to  guide  future  research  and  conclusions 
are  drawn. 

M PC-BASED  HAZARD  AVOIDANCE 

The  basic  principle  of  MPC  is  illustrated  in  Fig.  1.  At  time 
step  k ,  starting  from  the  sampled  initial  value,  an  optimal 
control  input  u  is  computed  by  solving  an  open-loop, 
constrained,  finite-time  optimal  control  problem  for  the 
prediction  horizon  p .  The  resulting  estimated  optimal  output 
V  satisfies  the  pre -defined  constraints  and  minimizes  a  cost 
function.  Only  the  first  value  of  the  computed  control  sequence 
is  applied.  Due  to  model  simplifications,  model  parameter 


FIGURE  1.  BASIC  PRINCIPLE  OF  MPC 
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FIGURE  2.  SCHEMATIC  OF  MPC-BASED  HAZARD 
AVOIDANCE  ALGORITHM 


uncertainties,  and/or  other  types  of  noise  and  uncertainties,  the 
actual  output  y  will  be  different  from  the  predicted  value. 
Therefore,  at  the  next  time  step  k  + 1,  the  optimal  control 
problem  is  solved  again  over  a  shifted  horizon  based  on  the 
new  state  measurements.  This  process  is  thus  iterated  at  each 
time  step. 

Figure  2  illustrates  at  a  high  level  how  MPC  can  be  utilized 
for  hazard  avoidance  purposes.  The  three  main  components  of 
the  MPC-based  hazard  avoidance  algorithm  are  the  vehicle 
model,  the  cost  function  and  constraints,  and  the  dynamic 
optimizer.  These  components  will  be  discussed  in  detail  below. 
The  outputs  of  the  hazard  avoidance  algorithm  are  the  control 
signals  for  the  UGV.  In  general,  they  are  the  steering  wheel 
angle,  throttle  and  brake  pedal  positions;  however,  for  the 
purposes  of  this  paper,  it  is  assumed  that  the  vehicle  speed  is 
constant  and  thus  only  the  steering  angle  is  considered  as  the 
control  signal.  The  UGV  will  in  reality  be  an  actual  vehicle 
platform;  however,  this  work  is  simulation  based  and  therefore 
the  UGV  is  represented  using  a  vehicle  model,  which  will  also 
be  described  below.  The  state  estimator  block  is  needed  to 
estimate  the  full  state  of  the  vehicle  from  the  control  signals 
and  measured  outputs  to  be  used  as  the  initial  condition  for  the 
vehicle  model  used  in  MPC.  In  this  work,  since  the  UGV  is 
simulated,  the  full  state  information  is  available  and  hence  the 
state  estimator  is  not  considered.  Finally,  the  information  about 
the  environment  is  assumed  to  be  obtained  using  a  L1DAR. 

The  rest  of  this  section  describes  the  vehicle  models,  cost 
function  and  constraints,  and  the  dynamic  optimizer  used  in  this 
work. 

Vehicle  Model 

There  are  two  places  in  the  general  schematic  in  Fig.  2 
where  a  vehicle  model  is  used  in  this  work.  To  validate  the 
hazard  avoidance  algorithm  in  simulation,  a  high-fidelity 
vehicle  model  is  needed  to  represent  the  UGV.  A  14  DoF 
representation  is  used  for  this  purpose  as  a  first  step  towards 
higher  fidelity  representations.  In  addition,  a  model  of  the 
vehicle  is  also  needed  in  the  MPC.  Towards  this  end,  this  work 
considers  two  different  representations:  a  2  DoF  model  and  the 
14  DoF  model.  Both  models  are  described  next. 


The  14  DoF  vehicle  model,  which  consists  of  a  single 
sprung  mass  connected  to  four  unsprung  masses,  is 
implemented  as  shown  in  Fig.  3  [19].  The  sprung  mass  is 
represented  as  a  plane  and  is  allowed  to  pitch,  roll,  and  yaw,  as 
well  as  displace  in  vertical,  lateral  and  longitudinal  directions. 
The  unsprung  masses  are  allowed  to  bounce  vertically  with 
respect  to  the  sprung  mass.  Each  wheel  is  also  allowed  to  rotate 
along  its  horizontal  axis  and  only  the  two  front  wheels  are  free 
to  steer.  In  summary,  this  model  consists  of  6  DoF  at  the 
vehicle  chassis  center  of  gravity  (CG),  and  2  DoF  at  each  of  the 
four  wheels,  including  vertical  suspension  travel  and  wheel 
spin. 

To  represent  the  tire  traction  forces,  the  lookup  table  based 
models  used  in  TRUCKSIM  [20]  are  adopted.  Nonlinear  tables 
are  used  to  represent  lateral  forces  and  aligning  moments  as  a 
function  of  slip  angle  and  vertical  load.  Another  nonlinear  table 
is  used  to  represent  longitudinal  forces  as  a  function  of  slip 
ratio  and  vertical  load.  Two-dimensional  linear  data 
interpolation  is  used  to  determine  the  forces  and  moment  for  a 
specific  combination  of  slip  and  load.  Combined  slip  effects  are 
not  considered. 

This  vehicle  model  is  parameterized  for  a  typical  4  wheel 
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truck  and  used  in  the  simulation  to  represent  the  real  system. 

The  standard  vehicle  model  used  in  the  state-of-the-art 
MPC-based  hazard  avoidance  algorithms  is  a  2  DoF  vehicle 
representation,  with  either  assuming  constant  cornering 
stiffness  or  augmenting  the  vehicle  model  with  Pacejka  Magic 
Formula  Tire  Model  [14-17].  Thus,  as  a  baseline,  a  2  DoF 
vehicle  model  is  also  used  in  this  study  with  a  constant 
cornering  stiffness  approach.  The  model  is  illustrated  in  Fig.  4. 

The  parameters  of  the  2  DoF  and  14  DoF  models  are  set 
identical  except  for  the  tire  parameters.  The  14  DoF  is  using  a 
tire  model  as  described  above,  whereas  the  2  DoF  model  is 
using  the  constant  cornering  stiffness  approach.  To  minimize 
the  difference  between  the  predictive  capabilities  of  the  two 
models,  the  cornering  stiffnesses  used  in  the  2  DoF  model  are 
tuned  to  match  the  response  of  the  2  and  14  DoF  models  as 
closely  as  possible.  Figures  5  and  6  compare  the  vehicle 
trajectories  and  lateral  accelerations,  respectively,  predicted  by 
the  two  models  under  different  steering  conditions.  The  results 
confirm  the  finding  in  the  literature  that  the  2  DoF  starts  losing 


FIGURE  5.  VEHICLE  TRAJECTORIES  FOR  20  M/S  VELOCITY 
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FIGURE  6.  LATERAL  ACCELERATIONS  FOR  20  M/S 
VELOCITY  PREDICTED  BY  THE  TWO  MODELS 


its  validity  after  a  lateral  acceleration  of  0.5  g  [15]. 

Cost  Function  and  Constraints 

The  cost  function  and  constraints  need  to  be  specified  to 
satisfy  the  objective  of  avoiding  the  locally  detected  obstacles 
while  guaranteeing  vehicle  safety  and  minimizing  the  travel 
time.  When  the  speed  of  the  vehicle  is  constant  as  in  this  study, 
minimizing  the  travel  time  is  the  same  as  minimizing  the  travel 
distance. 

Constraints.  The  constraints  represent  the  hard 
requirements  of  avoiding  collision  and  ensuring  vehicle  safety. 
These  requirements  are  hard  in  the  sense  that  their  violation  is 
not  allowed  under  any  circumstances. 

To  avoid  collision  with  the  obstacles,  a  minimum 
acceptable  distance  between  the  center  of  gravity  of  the  vehicle 
and  any  detected  points  by  the  LIDAR  sensor  (i.e.,  the 
boundaries  of  the  obstacles)  is  defined  and  set  to  3  meters. 

In  this  study,  ensuring  vehicle  safety  is  translated  to 
avoiding  tire  lift-off  for  the  14  DoF  model.  This  is  a 
conservative  criterion  used  to  prevent  rollover  [21].  It  is  also 
part  of  the  vehicle  acceptability  tests  used  in  the  military.  Note 
that  the  roll  angle  or  lateral  acceleration  threshold  can  also  be 
used  to  define  the  safety  of  a  vehicle  [22,  23].  Since  the  2  DoF 
cannot  predict  tire  lift-off,  a  maximum  lateral  acceleration  of 
0.5  g,  i.e.,  the  model’s  reported  range  of  validity  [15],  is  used  as 
the  safety  limit  for  the  2  DoF  model. 

As  shown  in  Fig.  7,  for  the  14  DoF  model  traveling  at  a 
constant  speed,  when  the  steering  angle  increases,  the  vertical 
tire  force  of  one  of  the  tires  becomes  smaller.  Hence,  there 
exists  a  maximum  steering  angle  such  that  the  minimum 
vertical  tire  force  is  close  to  zero  (4.3°  in  the  example  shown). 
When  the  steering  angle  increases  by  0.1°,  tire  lift-off  is 
observed  (4.4°  in  the  example  shown). 

Similarly,  the  maximum  allowable  lateral  acceleration 


FIGURE  7.  VERTICAL  TIRE  FORCE  FOR  VARIOUS 
STEERING  COMMANDS  AT  20  M/S  FOR  THE  14  DOF  MODEL 
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places  an  upper  limit  to  the  steering  angle  in  the  2  DoF  model. 
Figure  8  illustrates  how  lateral  acceleration  increases  with 
steering  angle. 

Note  that  the  maximum  steering  angle  is  a  function  of 
vehicle  speed.  There  exist  other  factors  that  can  change  the 
maximum  steering  angle,  for  example,  the  slope  of  the  terrain. 
Flowever,  in  this  study,  the  vehicle  is  assumed  to  move  on  a 
constant  friction  flat  surface.  Therefore,  the  maximum  steering 


angle  is  only  a  function  vehicle  speed.  For  speeds  ranging  from 
10  m/s  to  30  m/s,  the  relationship  between  the  maximum 
steering  angle  and  the  speed  is  shown  in  Fig.  9  for  both  the  2 
DoF  and  14  DoF  models.  This  also  corresponds  to  a  minimum 
turning  radius  as  shown  in  Fig.  10. 

In  summary,  the  two  hard  requirements  considered  in  this 
work  are  maneuvering  without  collision  and  without  tire  lift-off 
(14  DoF)  or  excessive  lateral  acceleration  (2  DoF).  For  the  first 
requirement,  obstacle  information  from  the  LIDAR  is  used.  The 
minimum  acceptable  distance  between  the  center  of  gravity  of 
the  vehicle  and  obstacles  is  set  to  3  meters.  For  the  second 
requirement,  if  the  steer  angle  is  below  the  maximum  allowable 
value  at  a  given  speed,  the  safety  of  the  vehicle  can  be 
guaranteed  when  the  vehicle  moves  on  a  constant  friction  flat 
surface.  Hence,  the  search  space  for  the  steering  angle  is 
limited  to  the  feasible  space. 

These  hard  requirements  are  implemented  as  hard 
constraints  in  the  sense  that  if  a  control  input  sequence  is 
predicted  to  cause  any  constraint  violation,  it  will  not  be 
considered  as  a  candidate. 


FIGURE  8.  LATERAL  ACCELERATION  FOR  VARIOUS 
STEERING  COMMANDS  AT  20  M/S  FOR  THE  2  DOF  MODEL 
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FIGURE  9.  MAXIMUM  STEERING  ANGLE  AS  A  FUNCTION 
OF  SPEED 
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Cost  Function.  The  cost  function  defines  the  soft 
requirement;  i.e.,  in  what  sense  the  trajectory  is  optimal.  Two 
terms  are  included  in  the  current  formulation  as  follows 

J  =  dg  (tn  +  nT)  +  wa(t0  +nT) 


where 


dg{t0  +  nT)=^J(x(t0  +  nT)-xg)2  +  (y(t0+nT)-yg)~ 

yg-y(h+nT) 


a[tQ  +nT)  =  tan  1 


xg-x(t0  +  nT) 


with  t0  representing  the  initial  time,  n  the  number  of 
prediction  steps,  T  the  sampling  time,  x  and  y  the 
position  of  the  UGV,  and  xg  and  y  the  position  of  the  goal. 


Planned 

Trajectory 


Obstacle 


a 

Start 


FIGURE  10.  MINIMUM  TURNING  RADIUS  AS  A  FUNCTION  FIGURE  11.  THE  COMPONENTS  OF  THE  COST  FUNCTION 

OF  SPEED 
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The  first  term  encourages  a  shorter  distance  between  the  end  of 
prediction  horizon  and  the  goal.  Due  to  the  constant  speed 
assumption  in  this  work,  this  term  also  aims  to  minimize  the 
remaining  travel  time.  Hence,  travel  time  is  not  explicitly 
included  in  the  cost  function.  The  second  term  penalizes  the 
deviation  of  the  heading  angle  from  the  goal  direction.  The  two 
terms  are  linearly  combined  using  a  relative  weight  w  .  Figure 
1 1  illustrates  components  of  the  cost  function. 

Dynamic  Optimizer 

Many  high  efficiency  optimization  algorithms  have  been 
considered  to  enable  real-time  operation  with  MPC.  Explicit 
methods  move  the  computational  burden  offline  and  store  the 
optimal  control  actions  in  a  look-up  table.  The  online 
computation  effort  can  then  be  reduced  to  locating  the 
measured  initial  state  in  the  polyhedral  partition  and  an  affine 
function  evaluation  [24].  There  are  also  fast  MPC  techniques 
such  as  move  blocking,  warm  starting,  or  an  early  termination 
of  an  appropriate  interior  point  method  [25-27].  Such 
approaches  will  be  considered  in  the  future  as  necessary.  In  this 
study,  as  a  first  step  to  validate  the  problem  formulation  and  the 
performance  of  defined  constraints  and  cost  function,  as  well  as 
the  performance  of  MPC  with  vehicle  models  of  different 
fidelity,  an  exhaustive  search  is  used  as  the  dynamic  optimizer. 

The  optimal  control  sequence  that  can  minimize  the  cost 
function  and  satisfy  the  constraints  is  selected  from  a  discrete 
steering  command  pool  of  [Smdx,-Smx  / 2,0,£max  / 2,/>'max ]  , 
where  Smsa  is  determined  based  on  Fig.  9.  Starting  from  the 
initial  state,  the  control  commands  from  the  pool  are  applied  to 
the  vehicle  and  the  resulting  trajectories  are  checked  for 
constraint  violation.  If  constraints  are  not  satisfied,  the  search 
branch  is  terminated.  Otherwise,  next  step  prediction  is 
performed  by  using  the  end  state  of  last  simulation  as  the  initial 
state  and  applying  all  the  steering  commands  from  the  pool. 
This  process  is  repeated  until  a  predetermined  number  of  steps 
in  the  prediction  horizon  is  reached.  Once  all  the  feasible  steps 
are  thus  determined,  their  costs  are  evaluated  and  the  control 


FIGURE  12.  ILLUSTRATION  OF  THE  EXHAUSTIVE  SEARCH 
METHOD  USING  A  STEERING  COMMAND  POOL  OF  THREE 
ELEMENTS 


FIGURE  13.  MPC-BASED  HAZARD  AVOIDANCE  AS 
IMPLEMENTED  IN  THIS  STUDY 

sequence  with  minimum  cost  is  considered  as  the  optimal 
control  sequence.  The  first  element  of  this  sequence  is  applied 
to  the  vehicle,  and  the  process  is  iterated  in  the  next  simulation 
step.  This  idea  is  illustrated  in  Fig.  12  with  a  steering  command 
pool  of  only  three  elements  for  clarity. 

Prediction  horizon  and  sampling  time  for  the  MPC  are 
selected  based  on  the  speed  of  the  vehicle,  the  minimum 
turning  radius  at  that  speed,  and  the  detection  range  of  the 
sensors  to  satisfy  the  following  inequalities 

U  n  -T  <D 


f^min  ZU-n-T  (1) 

Km+S<D 

where  U  is  the  speed,  D  is  the  LIDAR  detection  range,  Rmi„  is 
the  vehicle’s  minimum  turning  radius  and  S  is  the  safety  margin 
that  accounts  for  the  size  of  the  vehicle.  The  first  inequality 
simply  states  that  the  prediction  horizon  in  terms  of  the  traveled 
distance  is  limited  by  the  sensor  range.  The  second  inequality 
ensures  that  the  vehicle  can  make  a  90°  turn  when  necessary  to 
avoid  an  obstacle.  Finally,  the  last  inequality  ensures  that  there 
is  enough  distance  between  the  vehicle  and  the  obstacle  during 
a  90°  avoidance  maneuver. 

Figure  13  illustrates  how  Fig.  2  is  customized  for  the 
purposes  of  this  study. 

SIMULATION  RESULTS  AND  DISCUSSION 

To  validate  the  algorithm  and  study  the  effect  of  the 
complexity  of  the  vehicle  model  used  in  the  MPC,  two 
simulation  studies  are  conducted  with  two  different  obstacle 
maps.  The  speed  of  the  vehicle,  U  ,  is  fixed  to  be  20  m/s. 
The  number  of  steps  n  in  the  prediction  horizon  is  set  to  4 
steps.  Using  the  inequalities  (1),  the  sampling  time  T  is  set  to 
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TABLE  I.  SIMULATION  PARAMETERS 


TABLE  II.  PERFORMANCE  METRICS 


Parameter 

2  DoF 

Value 

14  DoF 

Vehicle  speed,  U 

20  m/s 

20  m/s 

Prediction  horizon  length,  n 

4  steps 

4  steps 

LIDAR  angular  range 

+90° 

±90° 

LIDAR  angular  resolution 

0.1° 

0.1° 

Weighting  parameter  w 

100 

100 

Maximum  steering  angle 

2.8° 

4.3° 

Minimum  turning  radius 

86.6  m 

63.7  m 

Planning  sampling  time,  T 

1.70  s 

1.25  s 

LIDAR  detection  range,  D 

136  m 

100  m 

Steering  angle  options 

±2.8°,±1.4°,0< 

5  ±4.3°,  ±2.15°,  0° 

1.25  s  for  the  14  DoF  model  and  1.70  s  for  the  2  DoF  model. 
The  LIDAR  detection  range  D  is  then  determined  to  be  100 
m  for  the  14  DoF  model  and  136  m  for  the  2  DoF  model.  At 
each  sampling  cycle,  the  obstacle  information  is  obtained  from 
the  LIDAR.  A  2D  LIDAR  is  assumed;  i.e.,  the  height  of  the 
obstacle  is  unknown.  The  LIDAR  returns  the  distance  between 
the  vehicle  and  the  closest  obstacle  boundaries  in  each  radial 
direction  at  an  angular  resolution  of  0.1°.  The  angular  range  is 
±90°  with  the  vehicle  heading  direction  being  the  0°  direction. 
No  noise  or  uncertainties  are  considered.  Simulations  are 
carried  out  using  variable  time  step  solvers  (ode45  for  the  2 
DoF  model,  and  ode23  for  the  14  DoF  model).  Table  I 
summarizes  the  parameters  used  in  the  simulation. 

Several  metrics  are  used  to  compare  the  performance  of  the 
system  using  the  two  different  vehicle  models  in  the  MPC.  One 
metric  is  the  total  time  used  to  reach  the  target.  This  is 
proportional  to  the  travel  distance  because  the  velocity  of  the 
vehicle  is  maintained  constant.  Another  metric  is  the  minimum 
distance  to  obstacles;  a  smaller  minimum  distance  means  the 
vehicle  moves  closer  to  the  boundary  of  the  obstacles.  Other 
metrics  include  the  maximum  roll  angle,  maximum  lateral 
acceleration,  average  roll  angle,  and  average  lateral 
acceleration.  Table  II  summarizes  the  performance  metrics  and 
Figs.  14  and  15  compare  the  trajectories  for  the  two  maps 
considered. 

The  results  with  the  14  DoF  model  confirm  that  the 
presented  formulation  can  successfully  navigate  the  vehicle 
through  the  obstacle  field  to  the  target.  For  Map  A,  MPC  with 
both  the  2  DoF  model  and  the  14  DoF  model  are  successful  in 
terms  of  avoiding  the  obstacles  safely.  When  the  14  DoF  model 
is  used  in  the  MPC,  however,  the  algorithm  performs  better  in 
terms  of  time  to  target  and  travel  distance.  This  is  due  to  the 
fact  that  MPC  with  the  14  DoF  model  uses  a  larger  steering 
limit  based  on  its  capability  to  predict  tire  lift-off. 

For  Map  B,  MPC  with  2  DoF  model  cannot  avoid  the 
obstacles  due  to  its  conservative  steering  limit  derived  from  its 
range  of  validity.  MPC  with  the  14  DoF  model  shows  that  it  is 
actually  possible  to  navigate  through  this  obstacle  field  safely  at 
the  given  speed  when  the  vehicle  is  operated  closer  to  its  limits. 


MAP  A  MAP  B 

2  DoF  14  DoF  2  DoF  14  DoF 

Time  to  target  (s) 

32.3 

29.4 

50.7 

Travel  distance  (m) 

641.2 

582.2 

1001.9 

Min  distance  to  obstacles  (m) 

8.46 

6.43 

14.4 

Max  roll  angle  (°) 

1.59 

2.11 

2.12 

Max  lateral  acceleration  (g) 

0.506 

0.684 

0.68 

Avg  roll  angle  (°) 

1.21 

1.34 

1.33 

Avg  lateral  acceleration  (g) 

0.390 

0.442 

0.43 

FIGURE  14.  VEHICLE  TRAJECTORIES  FOR  MAP  A 


x  (m) 

FIGURE  15.  VEHICLE  TRAJECTORIES  FOR  MAP  B 

It  is  very  important  to  note  that  these  results  do  not 
immediately  lead  to  the  conclusion  that  a  14  DoF  model  must 
be  used  in  the  MPC.  The  results  only  establish  the  importance 
of  operating  the  vehicle  close  to  its  limits  for  better 
performance.  In  the  results  so  far,  the  range  of  validity  of  the  2 
DoF  model  has  been  used  to  determine  the  steering  limits  for 
that  model,  which  prevented  the  MPC  to  push  the  vehicle  to  its 
limits.  A  different  scenario  is  also  conceivable  in  which  the 
safety  limits  are  established  using  the  14  DoF  model,  but  the 
MPC  still  utilizes  the  2  DoF  model.  The  simulations  for  the  two 
maps  are  repeated  using  this  scenario  and  the  results  are 
summarized  in  Table  III  and  Figs.  16  and  17. 

These  results  show  that  when  the  safe  steering  limits  are 
established  using  the  14  DoF  model,  the  performance  with  the  2 
DoF  model  can  be  very  similar  to  the  performance  with  the  14 
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TABLE  III.  PERFORMANCE  METRICS  WHEN  14  DOF 
MODEL’S  STEERING  LIMITS  ARE  USED  FOR  2  DOF  MODEL 


MAP  A  MAP  B 

2  DoF  14  DoF  2  DoF  14  DoF 


Time  to  target  (s) 

28.3 

29.4 

51.2 

50.7 

Travel  distance  (m) 

559.9 

582.2 

1013.0 

1001.9 

Min  distance  to  obstacles  (m) 

7.58 

6.43 

15.1 

14.4 

Max  roll  angle  (°) 

2.11 

2.11 

2.12 

2.12 

Max  lateral  acceleration  (g) 

0.684 

0.684 

0.68 

0.68 

Avg  roll  angle  (°) 

1.26 

1.34 

1.33 

1.33 

Avg  lateral  acceleration  (g) 

0.415 

0.442 

0.43 

0.43 

FIGURE  16.  VEHICLE  TRAJECTORIES  FOR  MAP  A  WHEN  14 
DOF  MODEL’S  STEERING  LIMITS  ARE  USED  FOR  2  DOF 
MODEL 


X  (m) 

FIGURE  17.  VEHICLE  TRAJECTORIES  FOR  MAP  B  WHEN  14 
DOF  MODEL’S  STEERING  LIMITS  ARE  USED  FOR  2  DOF 
MODEL 

DoF  model.  For  Map  A,  the  2  DoF  model  even  outperforms  the 
14  DoF  model  in  time  to  target.  The  comparable  responses  are 
likely  due  to  basic  idea  of  MPC  that  only  the  first  steering 
command  is  implemented  even  if  the  prediction  is  made  over 
several  steps.  The  prediction  errors  of  the  2  DoF  model 
accumulate  only  with  time;  hence  the  predictions  of  the  2  DoF 
and  the  14  DoF  models  are  closer  to  each  other  in  the  beginning 
of  the  prediction  horizon  than  at  the  end.  In  addition,  since 
MPC  is  repeated  at  every  time  step,  the  predictions  of  the  2 
DoF  are  corrected  at  each  time  step.  Hence,  2  DoF  model  might 
be  enough  for  the  purposes  of  MPC  under  some  conditions  as 


14  DoF  Front  Left  - 2  DoF  Front  Left 

14  DoF  Front  Right - 2  DoF  Front  Right 

14  DoF  Rear  Left  - 2  DoF  Rear  Left 

14  DoF  Rear  Right - 2  DoF  Rear  Right 


FIGURE  18.  VERTICAL  TIRE  FORCES  FOR  MAP  B  WHEN  14 
DOF  MODEL’S  STEERING  LIMITS  ARE  USED  FOR  2  DOF 
MODEL 


FIGURE  19.  LATERAL  ACCELERATIONS  FOR  MAP  B  WHEN 
14  DOF  MODEL’S  STEERING  LIMITS  ARE  USED  FOR  2  DOF 

MODEL 

long  as  the  safe  vehicle  operation  limits  are  established  using  a 
higher  fidelity  model. 

For  the  second  map,  Figures  18  and  19  show  the  vertical 
tire  forces  and  the  lateral  accelerations,  respectively.  From  Fig. 
18  it  can  be  seen  that  the  trajectories  with  both  the  2  DoF  and 
14  DoF  models  are  dynamically  safe  in  terms  of  tire  lift-off  and 
are  close  to  the  boundary.  Fig.  19  shows  that  the  maximum 
lateral  acceleration  when  the  vehicle  steers  at  4.3°  is 
approximately  0.65  g,  which  is  larger  than  0.5  g,  the  range  of 
validity  of  the  2  DoF  model.  Nevertheless,  the  2  DoF  still 
performs  well  because  of  the  abovementioned  reasons. 

This  work  has  the  following  limitations.  The  vehicle  speed 
is  assumed  to  be  constant  and  hence  steering  angle  is 
considered  as  the  only  control  input.  Even  though  this  reduces 
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the  control  input  search  space,  it  also  potentially  limits  the 
performance  of  the  algorithm.  It  also  limits  the  obstacle  fields 
that  the  UGV  can  go  through  safely  without  decelerating  and 
accelerating.  The  14  DoF  employed  in  this  study  is  certainly 
not  the  highest  fidelity  model  available,  but  it  was  used  in  this 
paper  as  a  first  step  towards  higher  fidelity  models.  Models 
derived  using  a  multibody  approach  [28]  can  potentially 
improve  the  performance  even  further.  It  is  also  reasonable  to 
expect  an  even  better  performance  from  higher  fidelity  models 
when  uneven  terrains,  more  realistic  tire -terrain  interactions, 
and  uncertainties  are  included  in  the  model.  Finally, 
computational  aspects  of  the  algorithm  need  to  be  evaluated 
and  solutions  for  a  real-time  implantation  using  high 
performance  computing  architectures  need  to  be  developed. 

CONCLUSIONS 

The  paper  presents  the  development  of  an  MPC-based 
framework  for  hazard  avoidance  in  large  UGVs  with  significant 
vehicle  dynamics.  Unlike  the  existing  MPC-based  approaches, 
the  framework  uses  only  local  information  about  the 
environment  available  from  onboard  sensors.  The  developed 
formulation  is  tested  in  simulation  using  a  14  DoF  model  of  a 
typical  4  wheel  truck  as  the  vehicle  platform  and  shows  that  the 
vehicle  can  navigate  safely  through  two  obstacle  fields  and 
reach  the  target. 

The  paper  further  investigates  the  effect  of  the  fidelity  of 
the  model  used  in  MPC  on  the  performance  of  the  hazard 
avoidance  maneuver,  where  performance  is  characterized 
mainly  by  the  time-to-target  metric.  Towards  this  end,  two 
different  models  are  used  in  the  MPC;  the  14  DoF  model  and  a 
2  DoF  representation.  The  2  DoF  model  is  the  typical  choice  in 
the  literature  for  MPC  and  hence  is  used  in  this  study  as  a 
benchmark.  The  results  show  that  the  2  DoF  does  not  give  the 
best  performance  when  the  steering  limits  are  based  on  the 
model’s  range  of  validity.  Due  to  this  conservative  approach, 
the  2  DoF  model  may  even  fail  to  navigate  the  vehicle  safely 
through  an  obstacle  field  that  can  actually  be  navigated  safely 
with  the  14  DoF  model.  Flowever,  the  2  DoF  model  can 
perform  comparable  to  the  14  DoF  model  when  the  safe 
steering  limits  are  derived  from  the  14  DoF  model. 

Flence,  the  conclusion  is  that  higher  fidelity  models  are 
needed  for  autonomous  navigation  to  push  the  vehicle  to  its 
limits  for  best  performance.  Flowever,  that  does  not  necessarily 
mean  that  the  high  fidelity  model  must  be  used  within  the 
MPC.  A  mixed-fidelity  approach  in  which  the  safe  operating 
limits  are  established  using  a  higher  fidelity  model  and  the 
MPC  is  performed  with  a  lower  fidelity  model  may  be  adequate 
for  satisfactory  results.  The  right  amount  of  fidelity  that  needs 
to  be  included  in  the  MPC  model  is  still  subject  to  future 
research. 
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